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[57] ABSTRACT 

A seven-degree-of-freedom robot arm with a six- 
degree-of-freedom end effector is controlled by a pro- 
cessor employing a 6-by-7 Jacobian matrix for defining 
location and orientation of the end effector in terms of 
the rotation angles of the joints, a 1 (or more)-by-7 
Jacobian matrix for defining 1 (or more) user-specified 
kinematic functions constraining location or movement 
of selected portions of the arm in terms of the joint 
angles, the processor combining the two Jacobian ma- 
trices to produce an augmented 7 (or more)-by-7 Jaco- 
bian matrix, the processor effecting control by comput- 
ing in accordance with forward kinematics from the 
augmented 7-by-7 Jacobian matrix and from the seven 
joint angles of the arm a set of seven desired joint angles 
for transmittal to the joint servo loops of the arms. One 
of the kinematic functions constrains the orientation of 
the elbow plane of the arm. Another one of the kine- 
matic functions minimizing a sum of gravitational 
torques on the joints. Still another one of the kinematic 
functions constrains the location of the arm to perform 
collision avoidance. Generically, one of the kinematic 
functions minimizes a sum of selected mechanical pa- 
rameters of at least some of the joints associated with 
weighting coefficients which may be changed during 
arm movement. The mechanical parameters may be 
velocity errors or position errors or gravity torques 
associated with individual joints. 
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CONFIGURATION CONTROL OF SEVEN 
DEGREE OF FREEDOM ARMS 

BACKGROUND OF THE INVENTION 5 

1. Technical Field 

The invention is related to the use of the configura- 
tion control method disclosed in U.S. Pat. No. 4,999,553 
by one of the inventors herein to the control of seven 
degree of freedom robot arms, using a forward kine- 
matic approach. 

2. Background Art 

U.S. Pat. No. 4,999,553, the disclosure of which is 
hereby incorporated herein by reference, discloses a ^ 
configuration control method employed in the present 
invention. 
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1. Introduction 

It has been recognized that robot arms with seven or 
more degrees-of-freedom (DOF) offer considerable 
dexterity and versatility over conventional six DOF 
arms [1]. These high-performance robot arms are kine- 
matically redundant since they have more than the six 
joints required for arbitrary placement of the end-effec- 
tor in the three-dimensional workspace. Kinematically 
redundant arms have the potential to approach the ca- 
pabilities of the human arm, which also has seven inde- 
pendent joint degrees-of-freedom [2], 

Although the availability of the “extra” joints can 
provide dexterous motion of the arm, proper utilization 
of this redundancy poses a challenging and difficult 
problem. Redundant manipulators have an infinite num- 
ber of joint motions which lead to the same end-effector 
trajectory. This richness in the choice of joint motions 
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complicates the manipulator control problem consider- 
ably. Typically, the kinematic component of a redun- 
dant manipulator control scheme must generate a set of 
joint angle trajectories, from the infinite set of possible 
trajectories, which causes the end-effector to follow a 5 
desired trajectory while satisfying additional con- 
straints, such as collision avoidance, servomotor torque 
minimization, singularity avoidance, or joint limit 
avoidance. Developing techniques to simultaneously 
achieve end-effector trajectory control while meeting 10 
additional task requirements is known as the redun- 
dancy resolution/problem, since the motion of the ma- 
nipulator joints must be “resolved” to satisfy both ob- 
jectives. 

Since redundancy is an important evolutionary step 15 
toward versatile manipulation, research activity in re- 
dundancy resolution and related areas has grown con- 
siderably in recent years, [e.g. 3-10]. For the most part, 
researchers have been working with a set of analytical 
tools based on linearized differential/kinematics mod- 20 
els. Previous investigations of redundant manipulators 
have often focused on local/optimization for redun- 
dancy resolution by using the Jacobian pseudoinverse 
to solve the instantaneous relationship between the joint 
and end-effector velocities. Redundancy resolution 25 
based on the Jacobian pseudoinverse was first proposed 
by Whitney [3] in 1969, and the null-space projection 
improvement was proposed by Liegeois [4] in 1977. 
Over the past two decades, most researchers have con- 
tinued to develop variations of the pseudoinverse ap- 30 
proach primarily because the complex nonlinear for- 
ward and inverse kinematics models have deterred fur- 
ther investigations into new redundancy resolution 
schemes. A conceptually simple approach to control of 
redundant manipulator configuration has been devel- 35 
oped recently based on augmentation of the manipula- 
tor forward kinematics [11]. This approach covers a 
wide range of applications and enables a major advance- 
ment in both understanding and developing new redun- 
dancy resolution methods. This paper presents the ap- 40 
plications of the configuration control approach to a 
large class of redundant industrial robot arms with 
seven degrees-of-freedom. 

The paper is organized as follows. Section 2 describes 
the kinematics of the 7 DOF Robotics Research arm 45 
and gives an overview of the configuration control 
approach. Various applications of the configuration 
control approach to the 7 DOF arm providing elbow 
control, collision avoidance, and optimal joint move- 
ment are given in Section 3. Section 4 describes the 50 
laboratory setup and the implementation of configura- 
tion control for real-time motion control of the 7 DOF 
arm, with elbow positioning for redundancy resolution. 
Conclusions drawn from this work are given in Section 
5. 55 


4 

The Robotics Research arm has an anthropomorphic 
design with seven revolute joints, as shown in FIG. 1 
and has nonzero offsets at all the joints. The arm is 
composed of a number of “modules” with roll and pitch 
motions. The shoulder joint with roll and pitch motions 
moves the upper-arm; the elbow joint with roll and 
pitch actions drives the forearm; and the wrist roll and 
pitch rotations together with the tool-plate roll move 
the hand. Essentially, the 7 DOF arm is obtained by 
adding the upper-arm roll as the 7th joint to a conven- 
tional 6 DOF arm design. The RR arm is supported by 
a pedestal at the base. 

For kinematic analysis of the RR arm, coordinate 
frames are assigned to the links in such a way that the 
joint rotation 0/ is about the coordinate axis z and the 
base frame {xo,yo>zo} is attached to the pedestal. The 
two consecutive frames {x^i,y/_i >2 /.i with origin O /-1 and 
{x/,y/,z/} with origin O/are related by the 4x4 homoge- 
neous transformation matrix [13] 


0) 

cos0/. — SIN0; 0 1 A/_i 

SIN0,COSa/_ i COS0 J COSa/_ i ~ SINa/_ i_ SINa/_ \d { 
SINtheta/SINa/_iCOStheta/SINa/_i COS a/_i COSai-i^- 

0 0 0 i 

where d a,-, and a / are the link length, joint offset and 
twist angle respectively, given in Table 1. The transfor- 
mation that relates the hand frame {7} to the base frame 
{0} is obtained as 




°Tj = 0 Ti A T 2 - 2 Cy^C 4 A C 5 - 5 C 6 - e C 7 = 


^0 0 0 . 1 j 


where R={r,y} is the 3x3 hand rotation matrix and 
p=[x, y, z] r is the 3x1 hand position vector with re- 
spect to the base. One common representation of the 
hand orientation is the triple roll-pitch-yaw Euler an- 
gles (p, 0, y). This three-parameter representation of 
hand orientation is subtracted from the hand rotation 
matrix R as follows [13]: 


p = Atan2(r^2, 03) 


( 3 ) 


2. Motion Control of 7 DOF Arms 
In this section, we describe the kinematics of the 7 
DOF Robotics Research arm under study and discuss 
the motion control of this arm using the configuration 
control approach. 60 

2.1 Kinematics of 7 DOF Robotics Research Arm 
The Robotics Research (RR) arm is one of the few 
kinematically-redundant manipulators that is commer- 
cially available at the present time [12]. The Model 
K1207 RR arm has been purchased by JPL and similar 65 
models by other NASA centers for research and devel- 
opment of technologies applicable to the NASA Space 
Telerobotics Projects. 


0 = Atsm2 ^ / 

y = Atanlinb Hi) 

where Atan2 is the two-argument arc tangent function, 
and it is assumed that the pitch angle 0 is not equal to or 
greater than ±90°. Therefore, the hand position and 
orientation can be described by the 6x 1 vector Y=[x, 
y, z, p, 0, y] r the three-dimensional workspace. 

The 6X7 Jacobian matrix J v relates the 6x1 hand 
rotational and translational velocity vector 
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* * 

to the lx 1 joint angular velocity vector 6 as V— J v 0. 
The hand Jacobian matrix is computed using the vector 
cross-product form [14] 

h h 

z\ X p 1 z\ X p 1 ■ . ■ zrj x p 1 J 

where z/is the unit vector along the z-axis of link frame 
{i}, and P * is the position vector from the origin Og of 
link frame {i}to the origin of hand frame {7}. The Jaco- 
bian matrix in (4) can be partitioned as 





(4) 


Jyr 
Jyt 

where J vr and J vt designate the rotational and transla- 
tional components of the Jacobian, that is, o)—J vr 6 and 
v—J vt $. In order to relate the joint velocities to the rate 
of change of the roll-pitch-yaw angles that represent the 
hand orientation, the rotational Jacobian J vr (4) is modi- 
fied to yield [13] 


fp \ / 0 -SINy COSyCOS/3 \ 

y = U 0 COSy SINyCOStf 

\y ) o -siw > 

where the transformation matrix II (5) 


(5) 

J yT d — irJyjQ 
maps a) to 





and det[II] = — cos/?=?^0 since /3^±90\ 

From (4) and (5), we obtain the 6x7 hand Jacobian 
matrix 


• 

I Tr/yr J 

* * m * 

which relates Y to 6 as Y —5^0)0. It is important to note 
that the computational efficiency can be creased signifi- 
cantly by exploiting the commonality of terms between 
the hand transformation matrix °T7 and the hand Jaco- 
bian matrix J e . 

Since the Robotics Research arm has seven joints, it 
offers one extra degree of joint redundancy for the task 
of controlling the six hand coordinates. The resolution 
of this single degree-of-redundancy is the subject of the 
next section. 

SUMMARY OF THE INVENTION 

A seven-degree-of-freedom robot arm with a six- 
degree-of-freedom end effector is controlled by a pro- 
cessor employing a 6-by-7 Jacobian matrix for defining 
location and orientation of the end effector in terms of 
the rotation angles of the joints, a 1 (or more)-by-7 
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Jacobian matrix for defining 1 (or more) user-specified 
kinematic functions constraining location or movement 
of selected portions of the arm in terms of the joint 
angles, the processor combining the two Jacobian ma- 
5 trices to produce an augmented 7 (or more)-by-7 Jaco- 
bian matrix, the processor effecting control by comput- 
ing in accordance with forward kinematics from the 
augmented 7-by-7 Jacobian matrix and from the seven 
joint angles of the arm a set of seven desired joint angles 
10 for transmittal to the joint servo loops of the arms. One 
of the kinematic functions constrains the orientation of 
the elbow plane of the arm. Another one of the kine- 
matic functions minimizing a sum of gravitational 
torques on the joints. Still another one of the kinematic 
15 functions constrains the location of the arm to perform 
collision avoidance. Generically, one of the kinematic 
functions minimizes a sum of selected mechanical pa- 
rameters of at least some of the joints associated with 
weighting coefficients which may be changed during 
0 arm movement. The mechanical parameters may be 
velocity errors or position errors or gravity torques 
associated with individual joints. 

BRIEF DESCRIPTION OF THE DRAWINGS 
25 

FIG. 1 is a perspective view of a seven degree of 
freedom robot arm of the type controlled in the present 
invention. 

FIG. 2 is a block diagram of an architecture embody- 
ing the present invention. 

FIG. 3 is a diagram of the robot arm of FIG. 1 in one 
position of interest. 

FIGS. 4a , 4b, 4c and 4 d are diagrams of the robot arm 
of FIG. 1 in various positions of interest. 

35 FIG. 5 is a diagram illustrating the coordinates em- 
ployed in the detailed description of the invention be- 
low. 

FIG. 6 is a graph illustrating the arm angle as a func- 
tion of the number of sampling steps in one implementa- 
40 tion of the invention. 

FIGS, la and lb are graphs illustrating joint angles of 
respective joints of the arm of FIG. 1 as a function of 
the number of sampling steps in an implementation of 
the invention. 

45 FIGS. 8a and 86 are graphs illustrating a collision 
weighting factor and a collision avoidance critical dis- 
tance, respectively, as a function of the number of sam- 
pling steps in an implementation of the invention. 

FIG. 9 is a graph illustrating the variation of the arm 
50 angle as a function of the number of sampling steps in an 
implementation of the invention. 

FIG. 10 is a graph illustrating various joint angles as 
a function of the number of sampling steps in an imple- 
mentation of the invention. 

55 FIG. 11 is a block diagram of a hardware system 
employed in carrying out one embodiment of the pres- 
ent invention. 

FIGS. 12a, 126, 12c, lid, lie, 12/ and 1 2g are graphs 
illustrating errors in respective parameters of joint posi- 
60 tion and joint angle as a function of time in an imple- 
mentation of the invention. 

DETAILED DESCRIPTION OF THE 
INVENTION 

65 2.2 Configuration Control of the 7 DOF Arm 

The configuration control approach introduced in 
[11] is a viable technique for resolution of redundancy 
and motion control of redundant manipulators. This 
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approach is based on redundancy resolution at the posi- 
tion (i.e., task) level through augmentation of the ma- 
nipulator forward kinematics by a set of user-defined 
kinematic functions <!>(0)={<}>i(0), • • • where r is 

the number of redundant manipulator joints. This is 5 
contrast to the conventional Jacobian pseudoinverse 
methods which resolve the redundancy at the velocity 
(i.e., differential kinematics) level. 

For the 7 DOF Robotics Research arm, the six hand 
position and orientation coordinates obtained in Section 10 
2.1 are augmented by the scaler user-defined kinematic 
function <f> to yield the 7x1 configuration vector 
X=[Y 7 ’,<f>] r . The redundancy resolution goal is then 
expressed as the additional task constraint 

that will be accomplished simultaneously with the basic 
task of controlling the hand motion Y(0)=Y<* (t), where 
<f>cKt) and Y^(t) are the desired time variations of <f>(0) 2 q 
and Y(0) respectively. Since the functional forms of the 
kinematic function and its desired time evolution are at 
the user’s discretion, this approach can accommodate a 
wide range of redundancy resolution goals such as arm 
posture control (i.e. elbow positioning [15]), satisfaction ^ 
of a task constraint (e.g. collision avoidance [16]), or L 
optimization of a kinematic performance measure (e.g. 
minimal joint movement [17]). This formulation puts 
the redundancy resolution on the same footing as the 
end-effector task, and treats them equally within a com- 
mon format. As a consequence, configuration control 5 
schemes ensure cyclicity (i.e., conservativeness) of arm 
motion, in contrast to pseudoinverse-based methods. 

The configuration control approach can be imple- 
mented either as a dynamic or a kinematic control law. 

In the dynamic control implementation [11], the config- 35 
uration controller produces the appropriate joint 
torques r(t) using a joint-space or a task-space formula- 
tion. In the kinematic control implementation [17], the 
controller generates the appropriate joint angle trajec- 
tories 0^(t) which are then used as setpoints for the 40 
low-level joint servo-loops. In this paper, we adopt the 
kinematic configuration control approach due to ease of 
implementation. Since the Robotics Research arm has 
non-zero joint offsets, there are no closed-form analyti- 
cal inverse kinematic solutions and therefore a differen- 45 
rial kinematics approach must be adopted. The aug- 
mented differential kinematics model of the arm is ob- 
tained as 
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ration control approach when the latter is implemented 
as a differential kinematic controller with an optimiza- 
tion additional task. Assuming det=0, equation (7) is 
solved in discrete-time as 

\e n )[x d {N+ l)~X(N)] (8) 

where N is the sampling instant, 0 and X are the actual 
values while O^and X^are the desired values. Note that 
the use of X in (8) corrects for linearization errors due 
to differential kinematics. The next desired joint angle is 
then computed from 0<*(N+l)=0d(N)-f A0^(N), and is 
sent as a setpoint to the joint servo-loops for tracking. 

The configuration control framework allows the user 
to specify multiple additional tasks to be accomplished 
simultaneously with the basic task of hand motion. Sup- 
pose that r(>l) additional task constraints are defined 
as <j> 2 <0)=<j>^-(t), i=l, . . . , r. Then, the augmented dif- 
ferential kinematics model becomes 


x d = 




( J > 
J e 


4>d 1 


Jcl 
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II 
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/ cr . 



( 9 ) 


The optimal (i.e. damped least-squares) solution of 
the over-determined set of equations (9) that has the 
smallest joint velocity ,|| 0 || is given by [17-19] as 

§=:[J T WJ+W V ]~ X J T WXd (10) 

or in discrete-time implementation 

WIX^N+D-Xm (11) 

where W=diag{W e ,W c } and W v are the (6+r)X(6-j-r) 
and 7x7 matrices of task error and joint velocity 
weighting factors specified by the user. Note that when 
W v =0, r=l and det[J]=,£0, equation (8) is retrieved 
from (10). The acquired solution (10) minimizes the 
scalar cost function 

L=E/W e E e +E c W c E c +^ r Wva (12) 






( 7 ) 


50 




m = mec) 


where J*(0) is the 6x7 hand Jacobian matrix obtained 
Section 2.1, 


55 


ad> 

W = -W 


60 


is the 1X7 Jacobian matrix associated with the kine- 
matic function <j>, and J(0) is the 7X7 augmented Jaco- 
bian matrix* Note that when <j>(0) is defined as the gradi- 
ent of an objective function to be optimized, J becomes 65 
the “extended” Jacobian proposed by Baillieul [9] for 
redundancy resolution. Therefore, the extended Jaco- 
bian method is retrieved as a special case of the configu- 


where I^=Yrf— J<?0 and Ec^^V— J^0 are the basic task 
and additional task velocity errors. The task weighting 
factors W e , W c enable the user to assign priorities to the 
different basic and additional task requirements. The 
joint velocity weighting factor W v allows the user to 
suppress large joint velocities near singularities, at the 
expense of small task errors. This is particularly impor- 
tant in redundant arm control because the complicated 
nature of the augmented Jacobian singularities deters 
any analytical characterization of the singular configu- 
rations. 

An architecture corresponding to Equations (8) and 
(10) is illustrated in FIG. 2. 

The ability to change the weighting factors on-line 
based on the task performance provides a general 
framework for incorporation of multiple constraints in 
redundant arm control. Equation (10) can be written as 
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e = ^ (13) 

+ jJwjc + _£ ; [* r »Wfc+ f Jj av^j] 

where J c / is the Jacobian related to <(>/. Equation (13) 
shows the contribution of each additional task con- 
straint to the optimal joint motion. This formulation can 
be used to “blend” multiple additional tasks or to io 
“switch” between different additional tasks by proper 
selection of their weighting factors. For instance, for 
the 7 DOF arm, we can switch between elbow control 
and collision avoidance during task execution so that 
when the arm is far from workspace obstacles, w >/= 1 15 
and w C 0 =O and direct elbow control will take prece- 
dence. As soon as potential collision is detected (from 
world model or sensory data), the collision avoidance 
goal becomes dominant and the corresponding 
weighting factor w co creases as the arm gets closer to 20 
the obstacle, at the expense of loss of direct elbow con- 
trol by setting w*/=0. This feature is illustrated in Sec- 
tion 3, and is discussed in detail in [17]. 

The configuration control formulation can be used to 
meet diverse additional task constraints for redundancy 25 
resolution [20]. For instance, the redundancy can be 1 
used to control directly: a geometrical variable (such as 
coordinates of a point on the arm), a physical variable 
(e.g. a joint gravity torque), or a mathematical function 
(such as projected gradient of an optimization function). 

In the next section, we demonstrate three applications 30 
of configuration control for motion control of the 7 
DOF Robotics Research arm. In each application, the 
single degree-of-redundancy is utilized to accomplish a 
different additional task objective; namely, elbow con- 
trol, collision avoidance, and optimal joint movement. 35 
3. Graphics Simulation of 7 DOF Arm Control 
This section describes some of the simulations of the 
configuration control scheme for redundancy resolu- 
tion and kinematic motion control of the Model K1207 
7 DOF Robotics Research arm. 40 

The Silicon Graphics IRIS 4D70-GT is a Worksta- 
tion with both high-speed computing and graphics ca- 
pabilities, and is used in this simulation study. A three- 
dimensional color rendering of the Robotics Research 
K1207 arm is built with a set of primitives that use the 45 
IRIS “C” language graphics library. When the program 
is run, it initially displays the arm and its state informa- 
tion on the IRIS screen as shown in FIG. 3 . The render- 
ing of the arm is centered on the screen with the joint 
angles, Cartesian hand coordinates, arm angle, manipu- 50 
lability indices, and trajectory time information dis- 
played in a table in the lower left comer, the redun- 
dancy control mode is displayed in the upper left, and 
the user menu box (not shown) appears as needed in the 
upper right comer of the screen. Since the zero configu- 55 
ration of this particular arm is a singular configuration, 
the arm shown in this figure is in the user-defined 
“home” configuration. Simulation software is written in 
“C” and animates the kinematic control results as they 
are computed so as to move the arm continuously on 60 
the screen. FIGS. 4 {a)-{d) show the evolution of the 
arm as it moves from an initial to a user-specified final 
configuration. Hie control law is computed and used to 
continuously change the arm configuration and the 
state information in the lower left comer of the screen is 65 
updated at every sampling instant. A simple cycloidal 
trajectory generator provides point-to-point straight- 
line Cartesian paths based on Cartesian goal points input 
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by the user either from the keyboard or from the mouse. 
Alternatively, the user may use the mouse in teleopera- 
tion mode to directly control the arm in joint or Carte- 
sian space, activating different degrees-of-freedom with 
the mouse buttons. Using a simple stacking feature, the 
user may save a sequence of intermediate points to a file 
for a later run. The user can also select from a number 
of redundancy resolution schemes for each task, adjust 
optimization parameters or obstacle location, plot the 
results of each run, or save the data for later analysis. 
The user may also rerun the simulation program, adjust- 
ing his viewing location and perspective on each run. 

This interactive graphics simulation environment 
serves as an essential tool for development and valida- 
tion of new control schemes for redundant 7 DOF arms. 
The IRIS also allows the user to simulate the robot 
workspace graphically and plan the task sequence. It 
can then be used for “task preview” by simulating the 
robot control algorithms and animating the task sce- 
nario. In this mode of operation, the IRIS can be used 
for operator training and rehearsal, prior to actual task 
execution. This preview mode is important in dealing 
with redundant arms, since it enables the user to explore 
various alternatives for redundancy resolution and can 
reveal unexpected behavior of the robot. 

Several configuration control schemes for the 7 DOF 
Robotics Research arm have been designed and verified 
by simulation on the IRIS. The case studies presented 
here are samples selected from an extensive computer 
simulation study which was carried out to test the per- 
formance of the proposed control schemes. These cases 
are chosen for presentation because they illustrate the 
flexibility and versatility of the configuration control 
approach to redundant manipulators. Three case studies 
are presented in this section, namely: elbow control, 
collision avoidance, and optimal joint movement. 

3.1 Elbow Control 

The presence of a redundant joint in the 7 DOF Ro- 
botics Research arm results in infinite distinct arm con- 
figurations with the same/hand position and orienta- 
tion. This leads to a physical phenomenon known as 
“self-motion” or “orbiting,” which is a continuous 
movement of the joints that leaves the hand motionless. 
The self-motion of the RR arm corresponds to the 
elbow point E traversing a circle around the line SW 
joining the shoulder S to the hand W, without moving 
the hand frame. Thus the elbow position, together with 
the hand coordinates, forms a complete representation 
of the geometrical posture (i.e., the physical shape) of 
the whole arm in the entire workspace. One natural 
representation of the elbow position is the “arm angle” 
m defined as the angle between the arm plane SEW and 
a reference plane, such as the vertical plane passing 
through the line SW, [15], as depicted in FIG. 5 . The 
angle ¥ succinctly characterizes the self-motion of the 
arm and uniquely specifies the elbow position for a 
given hand frame. Other viable representations of the 
elbow position are the x, y, or z Cartesian coordinates of 
the elbow (i.e., E x , E y, or E z ) in the base frame. The 
choice of 'P or a particular elbow coordinate is clearly 
dictated by the task that the arm is required to perform. 
In a recent paper [15], simple and computationally effi- 
cient methods of computing the arm angle ¥ and the 
associated constraint Jacobian Jy are given, where 
q>=Jvj,$. Following , ¥ and Jy are computed from 
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where E and W are the Jacobian matrices related to the 10 
elbow and the wrist linear velocities and other symbols 
are defined in FIG. 5, with ‘caret’ designating a unit 
vector. 

The user interacts with the IRIS Workstation by 
using the keyboard to enter the desired target position 15 
and orientation of the hand (x/,y/, z/, p/ } fif, yj) and the 
desired final arm angle ¥/, as well as the duration of 
motion r and the sampling period At. The hand frame 
can alternatively be input using the mouse which essen- 
tially emulates a 6 DOF cursor. The trajectory genera- 20 
tor software then computes smooth cycloidal trajecto- 
ries for these seven variables to change them from their 
initial values (xo, Yo, zo, po, fio, yo, ^o) to the final values 
in the specified time duration. For instance, a typical 
cycloidal trajectory for the desired arm angle ^ is 25 


\ 


Vf- ± o 

2tt 
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(16) 
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30 


Note that only the ratio of elapsed time to motion 
time r/t is needed for the trajectory generator. In dis- 35 
crete-time implementation, the number of samples dur- 
ing motion is equal to At/r. Note that, using the cycloi- 
dal functions, the hand moves on a straight-line path; 
since we obtain 

40 

* - *o _ y ~yo _ z - zo 

Xf-x o “ yf-yo - Zf-Zo 

In this simulation study, the Robotics Research arm is 
initially at the joint configuration 4S 
#(0)=:[—90%—43.3%0% — 101%— 180%— 54.3%— 90 0 ] 7, 

This yields the initial hand configuration 
P 0 ={X=0,y=90,z=0,p= -9O%0=O%y=O°} relative 
to the base frame and the initial arm angle ^o=0°, 
where the position coordinates are in centimeters and 5Q 
the angles are in degrees. The hand is commanded to 
trace a triangle by making the successive moves: 
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from 0° to —90° and then to 4-45° during the hand 
motion. The variations of the joint angles Q \, ...» to 
achieve the commanded arm motion are shown in 
FIGS, la-lb. These figures illustrate that all the seven 
joint angles return to their initial values at completion of 
the task. Thus, the initial and final arm configurations 
are identical and the robot has executed a cyclic (i.e., 
conservative) motion under configuration control. 

3.2 Collision Avoidance 

One of the advantages of the 7 DOF arm is the poten- 
tial to use the “extra” DOF to maneuver in a congested 
workspace and avoid collision with obstacles by config- 
uring the arm appropriately without perturbing the 
hand trajectory. In this formulation, all workspace ob- 
stacles are enclosed in convex volumes and each vol- 
ume defmes a “space of influence” (SOI) for the control 
law. In this study, the SOIs are assumed to be spheres in 
the three-dimensional workspace, but extension to other 
geometrical shapes is possible using distance functions 
[21]. In the configuration control framework, the colli- 
sion avoidance requirement is formulated as a kinematic 
equality/constraint 

(17) 

where d c (0)= || X c (0)— Xo |j is the critical distance be- 
tween the arm and the obstacle, Xois the position of the 
SOI center, ro is the radius of the SOI, and X c is the 
position of the “critical point” on the arm currently at 
minimum distance from the obstacle. Note that the 
location of the critical point X c and the critical distance 
d c are both configuration dependent and are continu- 
ously recomputed as described [16]. Two modes of 
operation are possible: 

Case One d c (0)=ro: * n this case, the equality con- 
straint (17) is satisfied and the entire arm is outside the 
obstacle SOI. Therefore, the constraint is active/and 
the manipulator redundancy can be used to achieve 
other additional tasks, such as those in Sections 3.1 and 
3.3. 

Case Two d c (0)<ro: In this case, the equality con- 
straint (17) is active/and the arm is inside the obstacle 
SOI. Thus, the redundancy is utilized to avoid collision 
with the obstacle by inhibiting the motion of the critical 
point on the arm in the direction toward the obstacle. 
To this end, (17) is replaced by the equality constraint 
d c (0)=ro, and the constraint Jacobian is obtained as 

_ ^6) 

M0) - d6 - • 


/>0>P. 55 

where 

Pi = {50, 50, 50, 0% 0% 0°}, ^=—90% n=2.0 
p 2== {_50, 50, 50, 0% 0% 0°}, * 2 = +45% r 2 =4.0, 
r 3 =1.0 60 

while At=0.025 in all cases and the unit of time is the 
second. The kinematic configuration control scheme is 
used to compute the required joint motions that result in 
the commanded hand and arm angle trajectories. Note 
that Jy from (15) is used in (10), and we set W e =l6, 65 
W c = 1 and W v =0 since no arm singularities are encoun- 
tered during the motion. FIG. 6 shows the executed 
motion of the elbow, in which the arm angle changes 


The configuration control scheme can now be em- 
ployed to achieve the desired hand motion as well as 
collision avoidance. However, in this formulation, the 
additional task constraint is either “on” or “off.” This 
can lead to an undesirable rapid switching between the 
“on” and “off’ conditions thus resulting a “chattering” 
phenomenon on the SOI boundary. Furthermore, 
switching between the collision avoidance task in Case 
Two and another additional task (such as elbow con- 
trol) in Case One can cause discontinuity problems. The 
variable task weighting scheme alleviates both the chat- 
tering and discontinuity problems. In this scheme, the 
weighting factors w e /and w^for the elbow control and 
collision avoidance tasks Cases One and Two are 
chosen as functions of the critical distance d^0) f instead 
of predefined constants. The use of variable weighting 
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factors for the additional tasks allows the collision 
avoidance constraint to be incorporated gradually with 
the basic task, and furthermore circumvents the discon- 
tinuity problem in switching between different addi- 
tional tasks. 

In the simulation study, the Robotics Research arm is 
initially at the joint configuration 0(O)=[— 90% —44.7°, 
0% -89.4% 0% -135.8% 90°F The task is to move the 
hand on a straight-line from the initial location Po=[0, 
90,0, 0% 0% 90°] to the target location Pi = [—90, 30, 
— 30, 0% 0% 90°] r=8.75 seconds with At=0.025 such 
that during motion the arm avoids collision with a 
workspace obstacle. The obstacle is enclosed by two 
SOIs: an inner SOI which touches the actual obstacle 
boundary, and an outer SOI which allows for some 
“buffer.” The inner and outer SOIs are concentric 
spheres with centers at zq=13.3] and radii r/=8.5 cm 
and ro=37.5 cm. Each hand coordinate is required to 
track a cycloidal trajectory as described in Section 3.1. 
Initially, before the obstacle is encountered, it is re- 
quired to keep the arm angle constant at its initial value 
of ^=0° to resolve the redundancy. When the obstacle 
is encountered, the redundancy is used for collision 
avoidance at the expense of loss of elbow control. After 
the obstacle encounter, the arm angle should remain 
constant. In this simulation, when the arm is outside the 
outer SOI (Case One), we set w e /=l and w co =0 to 
achieve elbow control. As soon as potential collision is 
detected (Case Two), the redundancy resolution goal 
switches smoothly to collision avoidance by setting 
w<?/=0 and increasing wco as an inverse square function 
of d/0), that is 

_ 10 _ 10 (18) 
{d c - r /) 2 (r a - lr /) 2 
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may also lead to a desirable distribution of joint torques 
for a given hand trajectory [17]. The condition for opti- 
mality of G(0) subject to the end-effector constraint 
Y=Y(0) has been found [17] to be 
5 

Nm = o, 

where N e is a 1 X7 vector which lies in the null-space of 
10 the hand Jacobian J e , that is J e N e T =0. This implies that 
for optimality, the projection of the gradient of the 
objective function onto the null-space of the hand Jaco- 
bian must be zero. To achieve optimal joint movement, 
the kinematic function is defined as 
15 

4>«?) = AW)-^L 

and its desired value is set to 4>/t)=0, to represent the 
20 optimality condition. The configuration control ap- 
proach can then be applied to obtain the joint trajecto- 
ries which cause the hand to attain the commanded 
motion with an optimal total joint spring energy. 

In this simulation study, the arm is initially at 
25 0(0)= [—89.1°, -32.1% -45% -91.5% -47% -126.6% 
29.7°] 71 giving the initial hand coordinates as Po=[50, 
70, 30, 0°, 0% 90°]. The hand is commanded to move on 
a straight-line to the target location Pi = [ — 50, 70, — 30, 
90°, 0% 0°] t= 2.5 seconds with At=0.025, while the 
u arm redundancy is used to achieve the hand trajectory 
with optimal joint movement. The user types in the 
stiffness coefficients of the joint springs 
{k :/} = {20, 1,1,1, 1,1,1}, where a large value for ki dictates 
35 the heavy penalty on joint 1 movement. The program 
then computes 


for d c ^ro- Using (18), when the arm is at the outer SOI 
boundary (d c — r c ), we have w co =0; and as the arm 
moves closer to the obstacle, w co creases rapidly so that 
Wco — *00 as dc-^r z % The variations of w co and the critical 
distance d/0) are shown in FIGS. Sa-Hb. It is seen that 
the increase in w co has hindered motion of the arm in- 
side the inner SOI, thus ensuring that collision avoid- 
ance is successfully accomplished throughout the arm 
motion. The variation of the arm angle ¥(0) is shown in 
FIG. 9, and illustrates that the arm angle is held con- 
stant when the obstacle is not encountered, as expected. 
3.3 Optimal Joint Movement 
In this case study, the redundancy resolution goal is 
to distribute the hand motion among the joints in such a 
way that a weighted sum of joint movements is kept at 
minimum. Toward this end, the optimization objective 
function is selected as 

7 ^ (19) 

= s o.sktfM - em 2 

i=l 

where k, is the weighting factor for joint i movement 
and [6 fit)— 0,(O)] 2 denotes the current deviation of joint 
angle Ofit) from its initial position 6 fid). The objective 
function G(0) (19) represents the total instantaneous 
potential energy stored in seven hypothetical springs 
attached to the robot joints with stiffness coefficients 
{k /} and natural lengths {0 ; {O)}. By choosing appropri- 
ate numerical values for {k/}, the user can resolve the 
hand motion among the joints such that the joints with 
larger k move less at the expense of those with smaller 
k. The ability to penalize individual joint movement 


<f>(9) = Ar e -|f-and/ c = -^- 
40 

and augments the hand Jacobian J e to obtain J. The 
required joint trajectories are then found by using (10) 
with W e — le, W c = 1, W v =0. The variations of the joint 
angles are given in FIG. 10, which shows that the first 
45 joint with a large weighting factor has moved consider- 
ably less than the other joints, as desired. 

3.4 Alternative Redundancy Resolution Goals 
In addition to the redundancy resolution goals dis- 
cussed in Section 3. 1-3.3, the user can select other crite- 
50 ria from a menu presented to him on the IRIS screen. 
This menu of redundancy resolution options is an area 
of current research, and analytical techniques that are 
being developed are added to the menu for test and 
validation. In this section, we shall present some of the 
55 items on the redundancy resolution menu. 

(i) Joint Locking: The user can select a particular 
joint, say Qj, to be locked during the commanded hand 
motion. In this case, the relationship 0/t)=0/O) is 
treated as the additional task, with J c = [0, ... ,1, ... 0]. 
60 The configuration control approach then attempts to 
move the hand using the remaining six joints while 
keeping 0j constant. This is equivalent to deleting the j 
th column of J e to obtain the 6x6 matrix J e and then 
solving Y = Jed for the remaining six joints 6. The ac- 
65 quired solution for 0 depends on the locked value of 0y, 
namely 0/0). Note /hat for some selections of 0y, the 
resulting Jacobian J e is always singular, which implies 
that from a physical point of view, the hand position 
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and orientation can not be changed arbitrarily while 6j 
is locked. The joint locking option is useful in investi- 
gating the fail-tolerance feature of the robot joints, i.e., 
preservation of hand motion despite a joint failure. In 
addition, this option can be exercised when the operator 5 
only wishes to perform the basic task of hand placement 
and orientation. 

(ii) Joint Limit Avoidance: The joints of any robot 

have rotational limitations that can typically be ex- 
pressed as where ay and /3yare the lower and 10 

upper joint limits. One of the applications of redun- 
dancy is to resolve the hand motion among the joints 
such that their limits are not violated. The joint limit 
equality constraint is treated within the configuration 
control framework in a similar manner to the obstacle 15 
avoidance constraint in Section 5.2. The user can select 
the joint limits and command hand motion, and examine 
the robot performance. Since inequality constraints are 
treated as equality conditions for redundancy resolu- 
tion, for some joint angles the augmented Jacobian can 20 
be singular and the problem may not have a solution. 

(iii) Manipulability Maximization: A common objec- 
tive function to be maximized by the utilization of re- 
dundancy is the hand manipulability index [15] defined 

as jx(0)— Vdet. This scalar index vanishes at the hand 25 
singular configurations where J e (0) is rank-deficient. 
Therefore, maximizing ja(0) during a prescribed hand 
motion leads to arm configurations which avoid the 
hand Jacobian singularities as much as possible. This 
solution can be obtained by following Section 5.3 with 30 
G(0) replaced by p,(0). Note that in this case must 
be computed numerically. 

4. Real-Time Control of the 7 DOF Arm 

In this section, we describe the implementation and 
experimental validation of the configuration control 35 
scheme on the 7 DOF Robotics Research arm. The 
laboratory setup is described first, followed by a de- 
scription of a simple experiment. In this experiment, the 
configuration control approach is implemented for real- 
time control of the Robotics Research arm, with elbow 40 
positioning for redundancy resolution. 

The Robotics Research Laboratory at JPL consists of 
one Model K1207 7 DOF arm and control unit from the 
Robotics Research Corporation, a VME-based chassis 
with MC 68020 processor boards, two 3 DOF industrial 45 
rate joysticks, a motorized lathe-bed, and a Silicon 
Graphics IRIS Workstation. The arm pedestal is 
mounted on a mobile platform of the lathe-bed which 
provides one additional degree-of-freedom. The arm 
control unit has an electronic servo-level interface, 50 
which allows the user to communicate directly with the 
joint servomotors at a sampling frequency of f s =400 
Hz, i.e., a sampling period of T s =2.5 ms. The joint 
servomotors can be commanded in any of the four 
modes: position, velocity, torque, and current. This 55 
makes it possible to operate the arm under either kine- 
matic or dynamic control schemes, and therefore pro- 
vides a testbed for validation of different 7 DOF control 
laws. In the present implementation, all seven joints are 
commanded in the position mode. 60 

The hardware diagram of the experimental setup is 
shown in FIG. 11 . The IRIS can operate in two differ- 
ent modes. First, it creates an interactive graphics simu- 
lation environment for analysis and control of the 7 
DOF arm, as discussed in Section 3. Second, the IRIS 65 
serves as the graphical user interface through which the 
operator interacts with the actual arm in real-time and 
issues motion commands in joint or task space. Using 
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this dual-mode functionality, the IRIS can be used ini- 
tially in “preview mode” for animating the task sce- 
nario, and subsequently in “execution mode” to com- 
mand the arm to duplicate the simulated motion. The 
software which provides graphical user interface and 
simulation capabilities resides on the IRIS. 

The VME-based real-time robot control system re- 
ceives commands from the IRIS to move the actual 
arm. This is the part of the system which handles all 
real-time operations including computation of control 
laws and transmission of appropriate signals to the mul- 
tibus-based arm control unit. The control unit dis- 
patches the commands for execution to the seven joint 
motors of the arm to perform the task. The VME chas- 
sis configuration contains five CPU boards that commu- 
nicate through a shared memory board to perform all 
the necessary computations to provide real-time manip- 
ulator control. The first CPU interfaces with the high- 
level software residing on the IRIS, receives commands 
from the operator and obtains acknowledgment and 
state information from the low level after command 
execution. This processor also serves as the master by 
scheduling the synchronous operations of the slave 
processors that perform the real-time computations. 
The second CPU performs real-time trajectory genera- 
tion and kinematic computations. This includes generat- 
ing the desired end-effector trajectories and computing 
the necessary kinematic and Jacobian transformations. 
The second CPU also accesses and updates the world 
model and performs computations to resolve the manip- 
ulator redundancy. The third CPU is designated to 
perform all the computations associated with invoking 
various dynamic control algorithms (not used at pres- 
ent). The fourth CPU solely communicates with the 
arm control unit by executing the arm interface driver 
at every 2.5 milliseconds. A two-card VME-to-multibus 
adaptor set from the BIT3 Corporation is employed to 
provide shared memory servo interface with the arm 
control unit at high speed. The role of the driver is to 
perform handshake with the arm control unit and to 
convert data into appropriate format for usage. Some of 
its features include translating data representation in the 
multibus to VMEbus format and vice versa and safety 
checking to avoid hitting physical joint limits and colli- 
sion with the floor. The fifth CPU hosts various drivers 
that manipulate the shared memory board which con- 
tains global memory formation, read in joystick inputs, 
control the motorized lathe-bed, and interface with 
other devices such as a force/torque sensor and a grip- 
per. All software executing on the VME environment is 
written in the “C” language. Code is developed on a 
SUN 3/60 UNIX computer utilizing SUN’s “C” com- 
piler and Wind River’s V x W orks/W ind real-time li- 
brary. The code is then downloaded through Ethernet 
to the target processor boards for immediate execution. 

To perform initial experiments, a computer program 
is written for trajectory generation, kinematic computa- 
tions, and arm interface via the driver. At the present 
time, all of these computations are performed sequen- 
tially on one MC68020 processor with a cycle period of 
25 milliseconds. First, a simple cycloidal Cartesian- 
space trajectory is generated based on the operator’s 
input of the desired arm goal configuration. The 7x1 
arm configuration vector X includes the 6x 1 vector Y 
of position and orientation coordinates of the hand and 
the scalar arm angle 't' for redundancy resolution. At 
each computation cycle, the output from the trajectory 
generator is the 7x1 vector of Cartesian increments 
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AX. The 7x7 augmented Jacobian J is also computed 
which embodies the redundancy resolution goal, 
namely ¥ control in this case. The Jacobian is then 
inverted and multiplied by the Cartesian increments to 
generate the seven joint increments A0=J _1 A X. Fi- 5 
nally, the joint setpoints are computed by adding the 
increments to the~ current joint angles and are dis- 
patched to the arm interface driver to move the arm 
under position mode. 

In the present implementation, because of the slow 10 
sampling rate of 25 milliseconds, the Jacobian matrix J 
is computed using the desired joint angles instead of the 
actual joint angles. In addition, the Cartesian increment 
AX is calculated using the difference between the two 
consecutive desired Cartesian setpoints, not by subtract- 15 
ing the actual Cartesian values from the desired Carte- 
sian setpoints. To improve performance, we plan to 
increase the servo rate by splitting the algorithm on two 
MC68020 processors. The first processor will be desig- 
nated solely to communicate with the arm at every 2.5 20 
milliseconds (running the driver as CPU 4 ). The second 
processor will perform cycloidal trajectory generation 
and Jacobian computation and version. The first proces- 
sor will then obtain the joint setpoints at every 25 milli- 
seconds, but will linearly interpolate these points into 25 
ten via-points which are then sent one at a time to the 
arm controller every 2.5 milliseconds. 

In the experiment, the Robotics Research arm is ini- 
tially at the predefined Cartesian “home” (“cstart”) 
position with the end-effector coordinates (x, y, z, roll, 30 
pitch, yaw) and arm angle ¥ as X(0) = [— 900, 297, 316, 

0% 0% 44°, 60°] measured relative to a fixed reference 
frame attached to the shoulder, where the lengths are 
millimeters and the angles are in degrees. This position 
corresponds to the joint angular values of 0(0)= [— 63% 35 
-61°, 78°,— 88°, 79°, -85°, 159°] which is away from 
the arm singular configuration. Data are collected as all 
seven Cartesian coordinates move simultaneously from 
the “cstart” position to the user-specified goal position 
X(r)=, where the motion duration r is chosen as 10 40 
seconds. This corresponds to the hand translational 
motion of 866 millimeters. Preliminary experimental 
results which demonstrate trajectory tracking are pres- 
ented in FIGS. 12a-12g. For each end-effector coordi- 
nate (x, y, z, roll, pitch, yaw) and the arm angle 'i', the 45 
tracking-error is computed by using the difference be- 
tween the actual trajectory and the desired trajectory. 
Note that the maximum error occurs in the middle of 
the trajectory, i.e. at time t=r/2=5 seconds. This is 
because for a cycloidal position trajectory, the velocity 50 
is at its peak in the middle of the trajectory, which 
attributes to the maximum occurrence of linearization 
errors. From FIGS. 12 a-g, in each positional coordi- 
nate, the maximum tracking-error does not exceed 16 
millimeters, and in each orientational coordinate, the 55 
maximum error is less than 3 degrees. Therefore, the 
experimental results demonstrate the efficacy of config- 
uration control for the 7 DOF arm. Note that the track- 
ing performance will be improved considerably when 
the computations are split on two processors so that the 60 
joint setpoints are updated every 2.5 milliseconds. 

5. Conclusions 

The problem of motion control of 7 DOF arms is 
addressed in this paper. To provide dexterous motion of 
the arm, the configuration control approach is adopted 65 
in which the redundancy in joint space is effectively 
transferred into task space by adding a user-defined 
kinematic constraint to the end-effector task. The con- 
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figuration control schemes are robust when singularities 
are encountered and allow the user to assign appropri- 
ate priorities to the task requirements. In this paper, 
applications of configuration control approach to mo- 
tion control of the 7 DOF Robotics Research arm are 
described. Diverse redundancy resolution goals such as 
elbow control, collision avoidance and optimal joint 
movement are demonstrated using computer graphics 
simulations. A simple laboratory experiment on config- 
uration control of the Robotics Research arm is de- 
scribed, and experimental results are presented. 

In contrast to Jacobian pseudoinverse methods which 
resolve the redundancy in joint space, the configuration 
control approach provides direct control of the manipu- 
lator in task space, where the task is performed. Fur- 
thermore, unlike pseudoinverse methods, the redun- 
dancy resolution goal is not restricted to optimization of 
a kinematic objective function. Finally, in contrast to 
pseudoinverse methods which do not ensure cyclicity 
of motion [22], the configuration control approach 
guarantees cyclic (i.e., conservative) motions of the 
manipulator, which is particularly important for repeti- 
tive tasks. By way of an example, in a 7 DOF arm under 
pseudoinverse control, the elbow is allowed to move 
without restraint during the hand motion, and the arm 
assumes different configurations for a closed-path hand 
movement [23]; whereas under configuration control, 
both of these problems are circumvented. 

Current work is focused on expanding the redun- 
dancy resolution goals, improving the computational 
efficiency, and performing further experiments on real- 
time motion control of the 7 DOF Robotics Research 
arm. 

While the invention has been described in detail by 
specific reference to preferred embodiments thereof, it 
is understood that variations and modifications may be 
made without departing from the true spirit and scope 
of the invention. 

What is claimed is: 

1. A method of controlling a robot arm, said robot 
arm comprising n joints, joint angle sensor means con- 
nected to each of said joints, joint servo loop means 
connected to each of said joints, and two ends compris- 
ing a fixed pedestal end and a movable hand end, each 
of said joints having a joint angle specifying a rotational 
orientation of said joint, said hand end having m degrees 
of freedom of movement, wherein m is "less than n 
whereby said robot arm characterized by a degree of 
redundancy, n-m, said method comprising: 

first defining a first m-by-n matrix for defining loca- 
tion and orientation of said hand end in terms of 
said rotation angles of said joints; 

second defining a second r-by-n matrix for defining r 
user-specified kinematic functions in terms of said 
joint angles, wherein r is a positive integer exceed- 
ing the degree of redun dancy of said robot arm; 

combining said first and second matrices to produce 
an augmented m+ r-by-n matrix; and 

computing in accordance with forward kinematics 
from said augmented m+ r-by-n matrix and from 
the joint angles sensed by said joint angle sensor 
means a set of n desired joint angles and transmit- 
ting said set of n desired joint angles to said joint 
servo loop means to control the robot arm. 

2. The method of claim 1 wherein each of said kine- 
matic functions defines a constraint in one of (a) loca- 
tion, (b) orientation and (c) configuration of a specified 
portion of said arm. 
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3. The method of claim 1 wherein said computing 
operates in successive iterations, said joint angles sensed 
by said joint angle sensor means correspond to a current 
one of said iterations and said set of n desired joint 
angles correspond to a next one of said iterations. 

4. The method of claim 1 wherein said first m-by-n 
matrix, second r-by-n matrix and augmented m+r-by-n 
matrix comprise Jacobian matrices. 

5. The method of claim 1 wherein said robot arm has 
an anthropomorthic structure and one of said joints is an 10 
elbow joint, wherein a pair of links of said arm joined 
together by said elbow joint define an elbow plane of 
said arm, and wherein one of said kinematic functions 
comprises means for defining an orientation of said 
elbow plane. 

6. The method of claim 1 wherein one of said kine- 
matic functions minimizes a sum of gravitational 
torques on at least some of said joints. 

7. The method of claim 1 wherein at least one of said 
kinematic functions constrains location of said arm. 

8. The method of claim 1 wherein at least one of said 
kinematic functions minimizes a sum of selected me- 
chanical parameters of at least some of said joints. 

9. The method of claim 8 wherein said parameters 
comprising different weighting coefficients for different 25 
ones of said joints. 

10. The method of claim 9 further changing said 
weighting coefficients during movement of said arm. 

11. The method of claim 8 wherein one of said me- 
chanical parameters comprises velocity errors of said 30 
joints with respect to desired velocities. 

12. The method of claim 8 wherein one of said me- 
chanical parameters comprises joint angle errors with 
respect to desired joint angles. 

13. The method of claim 5 wherein: 

a second one of said kinematic functions minimizes a 

sum of gravitational torques on at least some of said 
joints; 

a third one of said kinematic functions constrains 
location of said arm; and 

a fourth one of said kinematic functions minimizes a 
sum of selected mechanical parameters of at least 
some of said joints. 

14. The method of claim 1 wherein at least one of said 
user-specified functions corresponds to one of: 

i. joint locking; 

ii. joint limit avoidance; 

iii. manipulability m aximiz ation. 

15. A method of controlling a robot arm, said robot 
arm comprising n joints, joint angle sensor means con- 50 
nected to each of said joints, joint servo loop means 
connected to each of said joints, and two ends compris- 
ing a fixed pedestal end and a movable hand end, each 
of said joints having a joint angle specifying a rotational 
orientation of said joint, said hand end having m degrees 55 
of freedom of movement, wherein m is less than n 
whereby said robot arm characterized by a degree of 
redundancy, n-m, said method comprising: 

first defining a first m-by-n matrix for defining loca- 
tion and orientation of said hand end in terms of 60 
said rotation angles of said joints; 

second defining a second r-by-n matrix for defining r 
user-specified kinematic functions in terms of said 
joint angles, wherein r is a positive integer exceed- 
ing the degree of redundancy of said robot arm, 65 
and wherein at least one of said kinematic functions 
minimizes a sum of selected mechanical parameters 
of at least some of said joints and said selected 
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mechanical parameters include different weighting 
coefficients for different ones of said joints, said 
weighting coefficients being able to be changed 
during movement of said arm; 

5 combining said first and second matrices to produce 
an augmented m-f r-by-n matrix; and 
computing in accordance with forward kinematics 
from said augmented m+r-by-n matrix and from 
the joint angles sensed by said joint angle sensor 
means a set of n desired joint angles and transmit- 
ting said set of n desired joint angles to said joint 
servo loop means to control the robot arm. 

16. The method of claim 15 wherein said computing 
operates in successive iterations, said joint angles sensed 

15 by said joint angle sensor means correspond to a current 
one of said iterations and said set of* n desired joint 
angles correspond to a next one of said iterations. 

17. The method of claim 15 wherein said first m-by-n 
matrix, second r-by-n matrix and augmented m+r-by-n 

20 matrix comprise Jacobian matrices. 

18. The method of claim 15 wherein said mechanical 
parameters comprise velocity errors of said joints with 
respect to desired velocities. 

19. The method of claim 15 wherein said mechanical 
parameters comprise joint angle errors with respect to 
desired joint angles. 

20. A method of controlling a robot arm, said robot 
arm comprising n joints, joint angle sensor means con- 
nected to each of said joints, joint servo loop means 
connected to each of said joints, and two ends compris- 
ing a fixed pedestal end and a movable hand end, each 
of said joints having a joint angle specifying a rotational 
orientation of said joint, said hand end having m degrees 
of freedom of movement, wherein m is less than n 

35 whereby said robot arm characterized by a degree of 
redundancy, n-m, said method comprising: 
first defining a first m-by-n matrix for defining loca- 
tion and orientation of said hand end in terms of 
said rotation angles of said joints; 

40 second defining a second r-by-n matrix for defining r 
user-specified kinematic functions in terms of said 
joint angles, wherein r is a positive integer exceed- 
ing the degree of redundancy of said robot arm, 
and wherein at least one of said kinematic functions 
45 corresponds to one of, 

i. minimizing a sum of gravitational torques on at 
least some of said joints, 

ii. minimizing a sum of selected mechanical param- 
eters of at least some of said joints, said parame- 
ters comprising velocity errors of said joints 
with respect to desired velocities, and 

iii. minimizing a sum of selected mechanical param- 
eters of at least some of said joints, said parame- 
ters comprising joint angle errors with respect to 
desired joint angles; 

combining said first and second matrices to produce 
an augmented m+r-by-n matrix; and 
computing in accordance with forward kinematics 
from said augmented m+r-by-n matrix and from 
the joint angles sensed by said joint angle sensor 
means a set of n desired joint angles and transmit- 
ting said set of n desired joint angles to said joint 
servo loop means to control the robot arm. 

21. The method of claim 20 wherein said computing 
operates in successive iterations, said joint angles sensed 
by said joint angle sensor means correspond to a current 
one of said iterations and said set of n desired joint 
angles correspond to a next one of said iterations. 
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22. The method of claim 21 wherein said first m-by-n 
matrix, second r-by-n matrix and augmented m+r-by-n 
matrix comprise Jacobian matrices. 

23. The method of claim 21 wherein said selected 5 


10 


15 


20 


25 


30 


35 


40 


45 


50 


55 


60 


22 

mechanical parameters further include different 
weighting coefficients for different ones of said joints. 

24. The method of claim 23 further changing said 
weighting coefficients during movement of said arm. 

* * * * * 


65 



